#!/bin/bash

echo "==================== 简单里程计演示程序 ===================="

# 设置ROS环境
source /opt/ros/melodic/setup.bash
source ~/catkin_ws/devel/setup.bash

# 检查roscore是否运行
if ! pgrep -x "roscore" > /dev/null; then
    echo "错误: roscore未运行，请先启动roscore。"
    exit 1
fi

# 检查底盘节点是否运行
if ! pgrep -f "scout_mini_base.launch" > /dev/null; then
    echo "警告: 底盘节点未运行，尝试启动..."
    roslaunch scout_base scout_mini_base.launch port_name:=can0 simulated_robot:=false &
    sleep 5
fi

# 检查里程计话题是否存在
echo "检查里程计话题..."
rostopic list | grep -q "/odom"
if [ $? -eq 0 ]; then
    echo "✓ 里程计话题 /odom 存在"
else
    echo "✗ 里程计话题 /odom 不存在"
    echo "请先启动底盘节点："
    echo "roslaunch scout_base scout_mini_base.launch port_name:=can0 simulated_robot:=false"
    exit 1
fi

# 编译项目
echo "编译项目..."
cd ~/catkin_ws
catkin_make

if [ $? -eq 0 ]; then
    echo "✓ 编译成功"
else
    echo "✗ 编译失败"
    exit 1
fi

echo "启动简单里程计演示程序..."
echo "程序功能："
echo "1. 输入目标移动距离"
echo "2. 自动控制小车移动"
echo "3. 实时显示移动距离"
echo "4. 计算误差"
echo ""

# 启动演示程序
roslaunch scout_sensor_monitor simple_odometry_demo.launch
